/*
 * controlTask.c
 *
 *  Created on: 2024年11月4日
 *      Author: zizhong
 */
#include "foc_declare.h"
#include "Enum.h"
#include "trans.h"
#include "motor_para.h"
#include "measure.h"
#include "F2837xD_device.h"
#include "svpwm.h"
#include "board.h"
#include "sci.h"
#include "cur_control.h"
#include "PI.h"
#include "speed.h"

void runPosEst(MOTOR_Vars_t *pMotor);
void start(MOTOR_Vars_t *pMotor, float iq_cmd);

// Global variables used in this system

// motor drive variables
MOTOR_Vars_t motorVars;
extern uint16_t rDataA[11];

// Variables for Field Oriented Control
float VdTesting = 0.0;          // Vd reference (pu)
float VqTesting = 0.10;         // Vq reference (pu)

void buildLevel01(void)
{
    SCI_writeCharBlockingFIFO(SCIB_BASE, 0x1A);
    motorVars.speedRef = 0.03;
    motorVars.ctrlStateUse = CTRL_RUN;

    if(motorVars.ctrlStateUse == CTRL_RUN)
    {
        motorVars.rampController.TargetValue = motorVars.speedRef;
        motorVars.ipark.Ds = VdTesting;
        motorVars.ipark.Qs = VqTesting;
    }
    else
    {
        motorVars.rampController.TargetValue = 0;
        motorVars.rampController.SetpointValue = 0;
        motorVars.ipark.Ds = 0.0;
        motorVars.ipark.Qs = 0.0;
    }
    motorVars.fclPara.Vdcbus = getVdc(&motorVars);

    rampControlStep(&motorVars.rampController);

    motorVars.rampGen.Freq = motorVars.rampController.SetpointValue;
    rampGenerateStep((RAMP_gen_t *)&motorVars.rampGen);

    motorVars.ipark.Sine = __sinpuf32(motorVars.rampGen.Out);
    motorVars.ipark.Cosine = __cospuf32(motorVars.rampGen.Out);
    runIPark(&motorVars.ipark);

    motorVars.SVM.Ualpha = motorVars.ipark.Alpha;
    motorVars.SVM.Ubeta = motorVars.ipark.Beta;
//    runSVGenDQ(&motorVars.SVM, 24.0);

    EPWM_setCounterCompareValue(myEPWM0_BASE, EPWM_COUNTER_COMPARE_A,
                   (uint16_t)((M_INV_PWM_HALF_TBPRD * motorVars.SVM.Tc) +
                               M_INV_PWM_HALF_TBPRD));

    EPWM_setCounterCompareValue(myEPWM1_BASE, EPWM_COUNTER_COMPARE_A,
                   (uint16_t)((M_INV_PWM_HALF_TBPRD * motorVars.SVM.Tb) +
                               M_INV_PWM_HALF_TBPRD));

    EPWM_setCounterCompareValue(myEPWM2_BASE, EPWM_COUNTER_COMPARE_A,
                   (uint16_t)((M_INV_PWM_HALF_TBPRD * motorVars.SVM.Ta) +
                               M_INV_PWM_HALF_TBPRD));

}
float targetRPM = 400.0f;

void buildLevel02(void)
{

    motorVars.speedRef = targetRPM * 5 / 60.0 /motorVars.baseFreq;
    motorVars.alignCnt++;
//    motorVars.speedRef = 0.05;
//    motorVars.ctrlStateUse = CTRL_RUN;
//    motorVars.lsw = ENC_ALIGNMENT_DONE;
    if(motorVars.alignCnt >= motorVars.alignCntMax)
    {
        motorVars.lsw = ENC_ALIGNMENT_DONE;
    }
    else
    {
        motorVars.lsw = ENC_WAIT_FOR_ALIGNMENT;
    }


    if(motorVars.ctrlStateUse == CTRL_RUN)
    {
        motorVars.rampController.TargetValue = motorVars.speedRef;

        if(motorVars.lsw == ENC_ALIGNMENT_DONE)
        {
//            motorVars.ipark.Ds = 0;
//            motorVars.ipark.Qs = 2.0;
        }
        else if(motorVars.lsw == ENC_WAIT_FOR_ALIGNMENT)
        {
            motorVars.rampController.TargetValue = 0;
            motorVars.rampController.SetpointValue = 0;

//            motorVars.ipark.Ds = 0.5;
//            motorVars.ipark.Qs = 0.0;
        }
    }
    else
    {
        motorVars.rampController.TargetValue = 0;
        motorVars.rampController.SetpointValue = 0;
        motorVars.IdRef = 0;
        motorVars.cmplx_Id.ref = motorVars.IdRef;

        FCL_resetController(&motorVars);

        motorVars.curLoop.carryOut_d = 0;
        motorVars.curLoop.carryOut_q = 0;
        motorVars.curLoop.d_err = 0;
        motorVars.curLoop.q_err = 0;
        motorVars.curLoop.d_out = 0;
        motorVars.curLoop.q_out = 0;
        motorVars.curLoop.preWe = 0;
        motorVars.curLoop.we = 0;

        motorVars.spdLoop.integrator = 0;
        motorVars.spdLoop.iq_out = 0;
        motorVars.spdLoop.spdCnt = 0;
        motorVars.spdLoop.spd_err = 0;
        motorVars.spdLoop.zone = 0;

        motorVars.ipark.Ds = 0.0;
        motorVars.ipark.Qs = 0.0;
        motorVars.alignCnt = 0;
        motorVars.rampGen.Angle = 0;

    }

    start(&motorVars, motorVars.spdLoop.iq_out);

    //// to get the rotor position and speed
    runPosEst(&motorVars);


    rampControlStep(&motorVars.rampController);

    // Connect inputs of the RAMP GEN module and call the ramp generator module
    motorVars.rampGen.Freq = motorVars.rampController.SetpointValue;
    rampGenerateStep((RAMP_gen_t *)&motorVars.rampGen);


    motorVars.clarke.As =
    (float)ADC_readPPBResult(ADCBRESULT_BASE, ADC_PPB_NUMBER1) * 0.00024414 * (3.0f * 0.1f * 142.9f);
    //
    motorVars.clarke.Bs =
    (float)ADC_readPPBResult(ADCCRESULT_BASE, ADC_PPB_NUMBER1) * 0.00024414 * (3.0f * 0.1f * 142.9f);
    //
    motorVars.clarke.Cs =
    (float)ADC_readPPBResult(ADCARESULT_BASE, ADC_PPB_NUMBER1) * 0.00024414 * (3.0f * 0.1f * 142.9f);

//    runClarkeTwoInput(&motorVars.clarke);
    runClarkeThreeInput(&motorVars.clarke);

    //  Measure DC Bus voltage
//    motorVars.fclPara.Vdcbus = getVdc(&motorVars);

    motorVars.park.Alpha = motorVars.clarke.Alpha;
    motorVars.park.Beta   = motorVars.clarke.Beta;
//    motorVars.park.Angle  = motorVars.rampGen.Out;
    motorVars.park.Angle = motorVars.posElecTheta;
    motorVars.park.Sine = __sinpuf32(motorVars.park.Angle);
    motorVars.park.Cosine = __cospuf32(motorVars.park.Angle);
    runPark(&motorVars.park);

    //currLoop
    motorVars.curLoop.d_err = motorVars.IdRef - motorVars.park.Ds;
    motorVars.curLoop.q_err = motorVars.IqRef - motorVars.park.Qs;
    motorVars.curLoop.we = motorVars.speed.SpeedRpm / 60.0f;

    cur_PI(&motorVars.curLoop, motorVars.lsw);

    motorVars.ipark.Ds = motorVars.curLoop.d_out;
    motorVars.ipark.Qs = motorVars.curLoop.q_out;

    // Connect inputs of the INV_PARK module and call the inverse park module
    motorVars.ipark.Sine = motorVars.park.Sine;
    motorVars.ipark.Cosine = motorVars.park.Cosine;
    runIPark(&motorVars.ipark);

    // Connect inputs of the SVGEN_DQ module and call the space-vector gen. module
    motorVars.SVM.Ualpha = motorVars.ipark.Alpha;
    motorVars.SVM.Ubeta = motorVars.ipark.Beta;
    runSVGenDQ(&motorVars.SVM, 24.0);

    EPWM_setCounterCompareValue(myEPWM0_BASE, EPWM_COUNTER_COMPARE_A,
                   (uint16_t)((-M_INV_PWM_HALF_TBPRD * motorVars.SVM.Tc) +
                               M_INV_PWM_HALF_TBPRD));

    EPWM_setCounterCompareValue(myEPWM1_BASE, EPWM_COUNTER_COMPARE_A,
                   (uint16_t)((-M_INV_PWM_HALF_TBPRD * motorVars.SVM.Tb) +
                               M_INV_PWM_HALF_TBPRD));

    EPWM_setCounterCompareValue(myEPWM2_BASE, EPWM_COUNTER_COMPARE_A,
                   (uint16_t)((-M_INV_PWM_HALF_TBPRD * motorVars.SVM.Ta) +
                               M_INV_PWM_HALF_TBPRD));
    // Connect inputs of the SPEED_FR module and call the speed calculation module

    motorVars.absPos = (uint32_t)rDataA[2] | (uint32_t)rDataA[3] << 8 | (uint32_t)rDataA[4] << 16;
    motorVars.absPuTheta = motorVars.absPos * ABS_POS_PU_SCALE;
    motorVars.speed.ElecTheta = motorVars.posElecTheta;

    // Change motor speed for pu to rpm value
    // in->SpeedRpm = in->BaseRpm * in->Speed;
    runSpeedFR(&motorVars.speed);

    // Connect output speed of SPEED_FR to speedWe
    motorVars.speedWe = motorVars.speed.SpeedRpm;


    motorVars.spdLoop.spd_err = targetRPM - motorVars.speed.SpeedRpm;
    spd_PI(&motorVars.spdLoop, motorVars.lsw);

    SCI_writeCharBlockingFIFO(SCIB_BASE, 0x1A);
}


void initMotorFocPara(MOTOR_Vars_t *pMotor)
{
    pMotor->Ts = 0.001F / ((float)M_ISR_FREQUENCY);

    // set up current and voltage scaling coefficient
    pMotor->currentScale = M_CURRENT_SENSE_SCALE;
    pMotor->voltageScale = M_VOLTAGE_SENSE_SCALE;
    pMotor->adcScale = M_ADC_PU_SCALE_FACTOR;

    // Initialize the RAMPCTRL module
    // Maximum delay rate of ramp control
    pMotor->rampController.RampDelayMax = 1;
    pMotor->rampController.RampLowLimit = -1.0;
    pMotor->rampController.RampHighLimit = 1.0;

    // Initialize the RAMPGEN module
    pMotor->baseFreq = M_BASE_FREQ;
    pMotor->rampGen.StepAngleMax = pMotor->baseFreq * pMotor->Ts;
    pMotor->rampGen.Gain = 1.0;
    pMotor->rampGen.Offset = 1.0;

    pMotor->polePairs = M_POLES / 2.0;

    pMotor->voltageLimit = (float)(M_MAXIMUM_VOLTAGE);              // V
    pMotor->currentLimit = (float)(M_MAXIMUM_CURRENT);              // A
    pMotor->speedRefStart = (float)(M_STARTUP_FREQ/M_BASE_FREQ);    // PU
    pMotor->speedRefMax = (float)(M_MAXIMUM_FREQ/M_BASE_FREQ);

    pMotor->Vdcbus = M_MINIMUM_VOLTAGE;
    pMotor->VdcbusMax = M_MAXIMUM_VOLTAGE;
    pMotor->VdcbusMin = M_MINIMUM_VOLTAGE;



    // set mock REFERENCES for Speed and current loops
    pMotor->IdRef_start = M_ID_START;
    pMotor->IdRef_run = M_ID_RUN;
    pMotor->IdRef       = 0.0;
    pMotor->IqRef_start =   IQ_START;

    pMotor->offsetDoneFlag = 0;

    pMotor->fclPara.adcPPBScale = M_ADC_PPB_PU_SCALE_FACTOR;

    pMotor->offset_currentAs = 0.0;
    pMotor->offset_currentBs = 0.0;
    pMotor->offset_currentCs = 0.0;

    pMotor->fclPara.adcPPBScale = M_ADC_PPB_PU_SCALE_FACTOR;
    pMotor->fclPara.tSamp = (1.0F / M_SAMPLING_FREQ);
//    pMotor->fclPara.Rd    = M_RS;
//    pMotor->fclPara.Rq    = M_RS;
//    pMotor->fclPara.Ld    = M_LD;
//    pMotor->fclPara.Lq    = M_LQ;
//    pMotor->fclPara.BemfK = M_KB;

    pMotor->fclPara.Ibase = M_BASE_CURRENT;
    pMotor->fclPara.Wbase = 2.0 * PI * M_BASE_FREQ;
    pMotor->fclPara.wccD  = M_CUR_LOOP_BANDWIDTH;
    pMotor->fclPara.wccQ  = M_CUR_LOOP_BANDWIDTH;



    pMotor->IqRef = pMotor->IqRef_start;
    pMotor->IdRefDelta = 0.005;
    pMotor->IqRefDelta = 0.010;

    pMotor->maxModIndex = (M_TPWM_CARRIER -
            (2 * M_FCL_COMPUTATION_TIME)) / M_TPWM_CARRIER;
    pMotor->fclPara.carrierMid = pMotor->maxModIndex *
            M_INV_PWM_HALF_TBPRD * 0x10000L;

    // Initialize the Speed module for speed calculation from QEP/RESOLVER
    pMotor->speed.K1 = 1 / (pMotor->baseFreq * pMotor->Ts);

    // Low-pass cut-off frequency
    pMotor->speed.K2 = 1 / (1 + (2 * PI * pMotor->Ts * 5));
    pMotor->speed.K3 = 1 - pMotor->speed.K2;
    pMotor->speed.BaseRpm = (60.0F * pMotor->baseFreq) / pMotor->polePairs;

    // Init PI module for ID loop
//    pMotor->cmplx_Id.Kp = 1.0;         // LS * CUR_LOOP_BW;
//    pMotor->cmplx_Id.Ki = pMotor->Ts / 0.04;      // (RS * T) * CUR_LOOP_BW;
//    pMotor->cmplx_Id.Kerr = (pMotor->cmplx_Id.Ki * 0.5) + pMotor->cmplx_Id.Kp;
//    pMotor->cmplx_Id.KerrOld = (pMotor->cmplx_Id.Ki * 0.5) - pMotor->cmplx_Id.Kp;
//    pMotor->cmplx_Id.Umax = 0.5 * pMotor->maxModIndex;
//    pMotor->cmplx_Id.Umin = -0.5 * pMotor->maxModIndex;
//
//    // Don't need below commented code if memory is cleared by HAL_clearDataRAM
//    pMotor->cmplx_Id.ref = 0;
//    pMotor->cmplx_Id.err = 0;
//    pMotor->cmplx_Id.out = 0;
//
//    // Init PI module for IQ loop
//    pMotor->cmplx_Iq.Kp = pMotor->cmplx_Id.Kp;
//    pMotor->cmplx_Iq.Ki = pMotor->cmplx_Id.Ki;
//    pMotor->cmplx_Iq.Kerr =
//            (pMotor->cmplx_Iq.Ki * 0.5) + pMotor->cmplx_Iq.Kp;
//
//    pMotor->cmplx_Iq.KerrOld =
//            (pMotor->cmplx_Iq.Ki * 0.5) - pMotor->cmplx_Iq.Kp;
//
//    pMotor->cmplx_Iq.Umax = 0.8 * pMotor->maxModIndex;
//    pMotor->cmplx_Iq.Umin = -0.8 * pMotor->maxModIndex;
//
//    // Don't need below commented code if memory is cleared by HAL_clearDataRAM
//    pMotor->cmplx_Iq.ref = 0;
//    pMotor->cmplx_Iq.err = 0;
//    pMotor->cmplx_Iq.out = 0;

    pMotor->fclPara.cmidsqrt3 = pMotor->fclPara.carrierMid * __sqrt(3.0);

    pMotor->alignCntMax = ALIGN_CNT_MAX;
    pMotor->alignCnt = 0;

    pMotor->ctrlStateUse = CTRL_STOP;
}

void runPosEst(MOTOR_Vars_t *pMotor)
{
//    pMotor->alignCnt++;

    if(pMotor->lsw == ENC_ALIGNMENT_DONE)
    {
        if(pMotor->absPuTheta <= 0.14f)
        {
            pMotor->posMechTheta = pMotor->absPuTheta + 1.0F - 0.14f;
        }
        else
        {
            pMotor->posMechTheta = pMotor->absPuTheta - 0.14f;
        }

//        pMotor->posMechTheta = pMotor->absPuTheta;
        pMotor->posElecTheta = pMotor->polePairs  * pMotor->posMechTheta;
        pMotor->posElecTheta -= ((int32)(pMotor->posElecTheta));

        pMotor->pangle = pMotor->posElecTheta;
    }
    else if(pMotor->lsw == ENC_WAIT_FOR_ALIGNMENT)
    {


        pMotor->pangle = pMotor->rampGen.Out;
    }
}

void start(MOTOR_Vars_t *pMotor, float iq_cmd)
{
    if(pMotor->lsw == ENC_WAIT_FOR_ALIGNMENT)
    {
        pMotor->IdRef = 0.8f;
        pMotor->IqRef = 0.0f;
    }
    else if(pMotor->lsw == ENC_ALIGNMENT_DONE)
    {
        pMotor->IdRef = 0.0f;
        pMotor->IqRef = iq_cmd;
    }
}

